#include "rectifyFisheye.hpp"
#include "INIReader.h"
#include "ini.h"
#include "fisheye.hpp"
#include <iostream>
#include <string>
using namespace std;
using namespace cv;
using namespace xyVision;

xyVision::RectifyFisheye::RectifyFisheye(const string& configName)
{
    this->setParameters(configName);
}

void xyVision::RectifyFisheye::setParameters(const string& configName)
{
    INIReader reader(configName);
    string calibConfigName;
    const char* section = "cameraCamConf";
    const char* key = "calibPath";
    calibConfigName = reader.Get(section, key, " ");
    cv::FileStorage fs(calibConfigName, cv::FileStorage::READ);

    Mat temp;
    fs["camera_matrix"] >> temp;
    this->cameraMatrix = Matx33f(temp);

    fs["distortion_coefficients"] >> temp;
    temp = temp.t();
    this->distortCoeff = Matx14f(temp);

    key = "imageWidth";
    int width = reader.GetInteger(section, key, -1);
    key = "imageHeight";
    int height = reader.GetInteger(section, key, -1);
    this->imageSize = Size(width, height);

    section = "processConf";
    key = "scale";
    this->scale = (float)reader.GetReal(section, key, -1.0);
    this->newSize = Size(int(width*this->scale), int(height*this->scale));

    this->newCameraMatrix = Matx33f(cameraMatrix(0,0)*scale, 0.0f, newSize.width/2.0f,
        0.0f, cameraMatrix(0,0)*scale, newSize.height/2.0f,
        0.0f, 0.0f, 1.0f);

    cout << "new camera matrix" << endl;
    cout << this->newCameraMatrix << endl;

    xyVision::fisheye::initUndistortRectifyMap(this->cameraMatrix, this->distortCoeff,
        cv::Matx33d::eye(), this->newCameraMatrix, this->newSize, CV_16SC2, this->map1,
        this->map2);
}

void xyVision::RectifyFisheye::rectifyImg(const Mat& inputImg, Mat& outputImg)
{
    cv::remap(inputImg, outputImg, this->map1, this->map2, INTER_LINEAR, BORDER_CONSTANT);
}